/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief 静态地图层文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/static_layer.h>

using costmap_2d::FREE_SPACE;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;

namespace costmap_2d {
StaticLayer::StaticLayer(){};
StaticLayer::~StaticLayer() {}

void StaticLayer::onInitialize() {
  track_unknown_space_ =
      (*configNode_).getValueOfKey<bool>("track_unknown_space", false);
  use_maximum_ = (*configNode_).getValueOfKey<bool>("use_maximum", true);
  footprint_clearing_enabled_ =
      (*configNode_).getValueOfKey<bool>("footprint_clearing_enabled", true);

  int temp_lethal_threshold, temp_unknown_cost_value;
  temp_lethal_threshold =
      (*configNode_).getValueOfKey<int>("lethal_cost_threshold", 100);
  temp_unknown_cost_value =
      (*configNode_).getValueOfKey<int>("unknown_cost_value", 255);
  trinary_costmap_ =
      (*configNode_).getValueOfKey<bool>("trinary_costmap", false);

  lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
  unknown_cost_value_ = temp_unknown_cost_value;

  collect_signal_ = true;
  // 开一个线程，持续更新静态地图
  std::thread t1(&StaticLayer::getDatafromCollector, this);
  t1.detach();
}

void StaticLayer::matchSize() {
  // If we are using rolling costmap, the static map size is
  //   unrelated to the size of the layered costmap
  if (!layered_costmap_->isRolling()) {
    Costmap2D* master = layered_costmap_->getCostmap();
    resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(),
              master->getResolution(), master->getOriginX(),
              master->getOriginY());
  }
}

unsigned char StaticLayer::interpretValue(unsigned char value) {
  // check if the static value is above the unknown or lethal thresholds
  if (track_unknown_space_ && value == unknown_cost_value_) {
    return NO_INFORMATION;
  } else if (!track_unknown_space_ && value == unknown_cost_value_) {
    return FREE_SPACE;
  } else if (value >= lethal_threshold_) {
    return LETHAL_OBSTACLE;
  } else if (trinary_costmap_) {
    return FREE_SPACE;
  }

  double scale = (double)value / lethal_threshold_;
  return scale * LETHAL_OBSTACLE;
}

void StaticLayer::getDatafromCollector() {
  while (collect_signal_) {
    costmap_2d::OccupancyGrid new_map;
    data_collector_->getMap(new_map);

    unsigned int size_x = new_map.width, size_y = new_map.height;
    // resize costmap if size, resolution or origin do not match
    Costmap2D* master = layered_costmap_->getCostmap();
    if (!layered_costmap_->isRolling() &&
        (master->getSizeInCellsX() != size_x ||
         master->getSizeInCellsY() != size_y ||
         master->getResolution() != new_map.resolution ||
         master->getOriginX() != new_map.origin.position.x ||
         master->getOriginY() != new_map.origin.position.y)) {
      // Update the size of the layered costmap (and all layers, including this
      // one)
      layered_costmap_->resizeMap(size_x, size_y, new_map.resolution,
                                  new_map.origin.position.x,
                                  new_map.origin.position.y);
    } else if (size_x_ != size_x || size_y_ != size_y ||
               resolution_ != new_map.resolution ||
               origin_x_ != new_map.origin.position.x ||
               origin_y_ != new_map.origin.position.y) {
      // only update the size of the costmap stored locally in this layer
      resizeMap(size_x, size_y, new_map.resolution, new_map.origin.position.x,
                new_map.origin.position.y);
    }

    unsigned int index = 0;
    // initialize the costmap with static data
    for (unsigned int i = 0; i < size_y; ++i) {
      for (unsigned int j = 0; j < size_x; ++j) {
        unsigned char value = new_map.data[index];
        costmap_[index] = interpretValue(value);
        ++index;
      }
    }

    // we have a new map, update full size of map
    x_ = y_ = 0;
    width_ = size_x_;
    height_ = size_y_;
    map_received_ = true;
    has_updated_data_ = true;
  }
}

void StaticLayer::activate() { onInitialize(); }

void StaticLayer::deactivate() { collect_signal_ = false; }

void StaticLayer::reset() {
  collect_signal_ = false;
  onInitialize();
}

void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
                               double* min_x, double* min_y, double* max_x,
                               double* max_y) {
  if (!layered_costmap_->isRolling()) {
    if (!map_received_ || !(has_updated_data_ || has_extra_bounds_)) {
      return;
    }
  }

  useExtraBounds(min_x, min_y, max_x, max_y);

  double wx, wy;

  mapToWorld(x_, y_, wx, wy);
  *min_x = std::min(wx, *min_x);
  *min_y = std::min(wy, *min_y);

  mapToWorld(x_ + width_, y_ + height_, wx, wy);
  *max_x = std::max(wx, *max_x);
  *max_y = std::max(wy, *max_y);

  has_updated_data_ = false;

  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}

void StaticLayer::updateFootprint(double robot_x, double robot_y,
                                  double robot_yaw, double* min_x,
                                  double* min_y, double* max_x, double* max_y) {
  if (!footprint_clearing_enabled_) {
    return;
  }
  transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(),
                     transformed_footprint_);

  for (unsigned int i = 0; i < transformed_footprint_.size(); i++) {
    touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x,
          min_y, max_x, max_y);
  }
}

void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
                              int min_j, int max_i, int max_j) {
  if (!map_received_) {
    return;
  }
  if (!enabled_) {
    return;
  }
  if (!layered_costmap_->isRolling()) {
    // if not rolling, the layered costmap (master_grid) has same coordinates as
    // this layer
    if (!use_maximum_) {
      updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
    } else {
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
    }
  } else {
    // If rolling window, the master_grid is unlikely to have same coordinates
    // as this layer
    unsigned int mx, my;
    double wx, wy;
    // Might even be in a different frame

    for (unsigned int i = min_i; i < max_i; ++i) {
      for (unsigned int j = min_j; j < max_j; ++j) {
        // Convert master_grid coordinates (i,j) into global_frame_(wx,wy)
        // coordinates
        layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
        // Set master_grid with cell from map
        if (worldToMap(wx, wy, mx, my)) {
          if (!use_maximum_) {
            master_grid.setCost(i, j, getCost(mx, my));
          } else {
            master_grid.setCost(
                i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
          }
        }
      }
    }
  }
  if (footprint_clearing_enabled_) {
    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
  }
}

}  // namespace costmap_2d
